package org.beltrw.MazeSolver;

import lejos.nxt.*;
import lejos.nxt.ButtonListener;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.addon.CompassSensor;
import org.beltrw.Pilot.PinkRobotPilot;

/**
 * Example leJOS Project with an ant build file
 *
 */

public class MainSolver
{

    private static final int WHEEL_DIAMETER = 58;
    private static final int TRACK_WIDTH = 160;
    private static CompassSensor cs = new CompassSensor(SensorPort.S3);
    private static final float TRAVEL_DISTANCE = 393.7f;
    private static LightSensor ls = new LightSensor(SensorPort.S1);
    private static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S2);
    private static PinkRobotPilot pink;
    private static final int DIST_TO_WALL = 25;

    public static void main(String[] args)
    {
        Button.LEFT.addButtonListener(new KillSwitch());
        Button.RIGHT.addButtonListener(new KillSwitch());
        pink = new PinkRobotPilot(Motor.A, Motor.C, cs, TRACK_WIDTH, WHEEL_DIAMETER, true, ls, us);
        solveMazeRightHand();
//        pink.turnRight90();
//        LCD.drawString(""+cs.getDegreesCartesian(), 0, 0);
//        Button.waitForPress();
//        pink.turnLeft90();
//        LCD.drawString(""+cs.getDegreesCartesian(), 0, 1);
//        Button.waitForPress();
//        pink.turnLeft90();
//        LCD.drawString(""+cs.getDegreesCartesian(), 0, 2);
//        Button.waitForPress();
    }

    /**
     * Solves a maze using the Right Hand on wall algorithm
     */
    public static void solveMazeRightHand()
    {
        // keep going until on black space
        while(true /*!onBlack()*/)
        {
            takeRightmostPath();
        }
    }

    /**
     * Solves a maze using the Left Hand on wall algorithm
     */
    public static void solveMazeLeftHand()
    {
        // keep going until on black space
        while(true /*!onBlack()*/)
        {
            takeLeftmostPath();
        }
    }

    /**
     * Method to take right-most path. First, determines right-most path. Then
     * rotates the robot if needed. Finally, travels forward 1 cell.
     * If stuck in a corner, robot will turn 180 and go back 1 cell.
     */
    private static void takeRightmostPath()
    {
        int[] walls = pink.getSurroundings(Motor.B); // get locations of walls in current cell
        if(walls[0] > DIST_TO_WALL)
        {
            // turn right and go
            pink.turnRight90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else if (walls[1] > DIST_TO_WALL)
        {
            // go forward
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else if (walls[2] > DIST_TO_WALL)
        {
            // turn left and go
            pink.turnLeft90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else
        {
            // turn 180 and go
            pink.turnRight90();
            pink.turnRight90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
    }

    /**
     * Method to take left-most path. First, determines left-most path. Then
     * rotates the robot if needed. Finally, travels forward 1 cell.
     * If stuck in a corner, robot will turn 180 and go back 1 cell.
     */
    private static void takeLeftmostPath()
    {
        int[] walls = pink.getSurroundings(Motor.B); // get locations of walls in current cell
        if(walls[2] > DIST_TO_WALL)
        {
            // turn right and go
            pink.turnLeft90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else if (walls[1] > DIST_TO_WALL)
        {
            // go forward
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else if (walls[0] > DIST_TO_WALL)
        {
            // turn left and go
            pink.turnRight90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
        else
        {
            // turn 180 and go
            pink.turnLeft90();
            pink.turnLeft90();
            pink.forwardACell(TRAVEL_DISTANCE);
        }
    }

    static class KillSwitch implements ButtonListener
    {
        public void buttonPressed(Button b)
        {
            Sound.beepSequence();
            System.exit(0);
        }

        public void buttonReleased(Button b) {}
    }

}
